#include <rclcpp/rclcpp.hpp>
#include <mosquitto.h>
#include <iostream>
#include "std_msgs/msg/int32_multi_array.hpp"
#include "jsoncpp/json/json.h"

class MqttNode : public rclcpp::Node
{
public:
    MqttNode() : Node("mqtt_node1")
    {
        mqtt_count = 0;
        ros_count = 0;
        ros_publisher_ = this->create_publisher<std_msgs::msg::Int32MultiArray>("mqtt_", 10);
    }

    void connect()
    {
        // 初始化 Mosquitto 库
        mosquitto_lib_init();
        // 创建 MQTT 客户端实例
        mqtt_client = mosquitto_new(nullptr, true, this);
        if (!mqtt_client)
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to create MQTT client");
            return;
        }
        // 设置回调函数
        mosquitto_connect_callback_set(mqtt_client, on_connect);
        mosquitto_message_callback_set(mqtt_client, on_message);

        // 连接到 MQTT Broker
        if (mosquitto_connect(mqtt_client, "localhost", 1883, 60) != MOSQ_ERR_SUCCESS)
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to connect to MQTT Broker");
            return;
        }

        // 订阅主题
        mosquitto_subscribe(mqtt_client, nullptr, "test_topic", 0);

        // 启动循环以处理MQTT消息
        mosquitto_loop_start(mqtt_client);

        // 定时器发送示例消息
        timer_ = this->create_wall_timer(std::chrono::seconds(2), std::bind(&MqttNode::publish_message, this));
    }

    ~MqttNode()
    {
        mosquitto_destroy(mqtt_client);
        mosquitto_lib_cleanup();
    }

private:
    void publish_message()
    {
        // int value = 42; // 示例整型数据
        ros_count++;
        Json::Value json_val;
        json_val["ros_count"] = ros_count;
        json_val["x"] = 0.1f;
        json_val["y"] = 0.2f;
        publish("upload_pos1", json_val);
    }

    void publish(const std::string &topic, const std::string &msg)
    {
        if (mosquitto_publish(mqtt_client, nullptr, topic.c_str(), msg.size(), static_cast<const void *>(msg.c_str()), 0, false) != MOSQ_ERR_SUCCESS)
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to publish message");
        }
        else
        {
            // RCLCPP_INFO(this->get_logger(), "Message published: %d", value);
        }
    }

    void publish(const std::string &topic, const Json::Value& json_val)
    {
        publish(topic, json_val.toStyledString());
    }

    static void on_connect(struct mosquitto *client, void *userdata, int rc)
    {
        auto node = static_cast<MqttNode *>(userdata);
        if (rc == 0)
        {
            RCLCPP_INFO(node->get_logger(), "Connected to MQTT Broker");
        }
        else
        {
            RCLCPP_ERROR(node->get_logger(), "Failed to connect to MQTT Broker");
        }
    }

    static void on_message(struct mosquitto *client, void *userdata, const struct mosquitto_message *message)
    {
        auto node = static_cast<MqttNode *>(userdata);
        if (node)
        {
            node->onMessage(std::string(message->topic), std::string((char *)message->payload, message->payloadlen));
        }
    }

    void onMessage(const std::string &topic, const std::string &msg)
    {
        // RCLCPP_INFO(get_logger(), "recv topic: %s, msg: %s", topic.c_str(), msg.c_str());
        try
        {
            Json::Reader reader;
            Json::Value json_val;
            if(reader.parse(msg, json_val))
            {
                
                RCLCPP_INFO(get_logger(), "recv topic: %s, msg: %s", topic.c_str(), json_val.toStyledString().c_str());
                int cmd = json_val["cmd"].asInt();
                if(cmd == 0)
                {
                    double x = json_val["pose"]["x"].asDouble();
                    double y = json_val["pose"]["y"].asDouble();
                    double yaw = json_val["pose"]["yaw"].asDouble();
                    RCLCPP_INFO(get_logger(), "pose: %.3f, %.3f, %.3f", x, y, yaw);
                }
            }
            else  RCLCPP_INFO(get_logger(), "receive topic:%s ,msg='%s'",topic.c_str(),msg.c_str());
        }
        catch(const std::exception& e)
        {
            std::cerr << e.what() << '\n';
        }
        
    }

    struct mosquitto *mqtt_client;
    rclcpp::TimerBase::SharedPtr timer_;
    rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr ros_subscription_;

    rclcpp::Publisher<std_msgs::msg::Int32MultiArray>::SharedPtr ros_publisher_;
    int32_t from_ros[40], from_mqtt[40];
    int32_t ros_count, mqtt_count;
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<MqttNode>();
    node->connect();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}